#!/usr/bin/env python

PACKAGE='mrs_pcl_tools'
import roslib;
roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator();

initial_registration_method = gen.enum([
                                gen.const("FPFH_init", int_t, 0, "Fast Point Feature Histogram"),
                                gen.const("NDT_init", int_t, 1, "Normal Distribution Transform"),
                                gen.const("GICP_init", int_t, 2, "Generalized Iterative Closest Point"),
                                gen.const("ICPN_init", int_t, 3, "Iterative Closest Point with Normals"),
                                gen.const("SICPN_init", int_t, 4, "Sampled Iterative Closest Point with Normals")],
                                "Registration method")

fine_tuning_registration_method = gen.enum([
                                gen.const("None", int_t, -1, "No fine tuning"),
                                gen.const("FPFH_fine", int_t, 0, "Fast Point Feature Histogram"),
                                gen.const("NDT_fine", int_t, 1, "Normal Distribution Transform"),
                                gen.const("GICP_fine", int_t, 2, "Generalized Iterative Closest Point"),
                                gen.const("ICPN_fine", int_t, 3, "Iterative Closest Point with Normals"),
                                gen.const("SICPN_fine", int_t, 4, "Sampled Iterative Closest Point with Normals")],
                                "Registration method")

reg = gen.add_group("A Registration parameters");
reg.add("init_reg_method", int_t, 0, "Initial registration method", 4, 0, 4, edit_method=initial_registration_method);
reg.add("fine_tune_reg_method", int_t, 0, "Fine tuning registration method", 3, -1, 4, edit_method=fine_tuning_registration_method);
reg.add("use_init_guess", bool_t, 0, "Use init guess? [true/false]", False)
reg.add("init_guess_yaw", double_t, 0, "Heading of initial guess of registration", 0.0, -3.1416, 3.1416);
reg.add("clouds_voxel_leaf", double_t, 0, "Voxel leaf size", 0.3, 0.0, 10000.0);
reg.add("norm_estim_rad", double_t, 0, "Normals estimation radius", 0.25, 0.0, 100.0);
reg.add("cloud_correlation_z_crop_offset", double_t, 0, "Z-axis offset in target cloud cropping after clouds correlation", 2.0, -10000.0, 10000.0);
reg.add("min_convergence_score", double_t, 0, "Minimal score thresholding successfull corvengence", 0.5, 0.0, 10000.0);

fpfh = gen.add_group("B FPFH registration parameters");
fpfh.add("fpfh_search_rad", double_t, 0, "Features search radius", 3.5, 0.0, 10000.0);
fpfh.add("fpfh_similarity_threshold", double_t, 0, "Polygonal edge length similarity threshold", 0.9, 0.0, 10000.0);
fpfh.add("fpfh_inlier_fraction", double_t, 0, "Required inlier fraction for accepting a pose hypothesis", 0.05, 0.0, 10000.0);
fpfh.add("fpfh_ransac_max_iter", int_t, 0, "Number of RANSAC iterations", 50000, 0, 2147483646);
fpfh.add("fpfh_number_of_samples", int_t, 0, "Number of points to sample for generating/prerejecting a pose", 4, 0, 2147483646);
fpfh.add("fpfh_corr_randomness", int_t, 0, "Number of nearest features to use", 14, 0, 2147483646);

ndt = gen.add_group("C NDT registration parameters");
ndt.add("ndt_transformation_epsilon", double_t, 0, "NDT transformation epsilon", 0.01, 0, 10000000.0);
ndt.add("ndt_step_size", double_t, 0, "NDT step size", 0.5, 0, 10000000.0);
ndt.add("ndt_resolution", double_t, 0, "NDT resolution", 1.0, 0, 10000000.0);
ndt.add("ndt_maximum_iterations", int_t, 0, "NDT maximum iterations", 100, 0, 2147483646);

gicp = gen.add_group("D GICP registration parameters");
gicp.add("gicp_max_corr_dist", double_t, 0, "GICP maximum correspondence distance", 5.0, 0.0, 10000000.0);
gicp.add("gicp_ransac_outl_rej_thrd", double_t, 0, "GICP RANSAC outlier rejection threshold", 0.3, 0.0, 10000000.0);
gicp.add("gicp_trans_eps", double_t, 0, "GICP transformation epsilon", 0.1, 0.0, 10000000.0);
gicp.add("gicp_max_iter", int_t, 0, "GICP maximum iterations", 50, 0, 2147483646);
gicp.add("gicp_max_opt_iter", int_t, 0, "GICP optimizer maximum iterations", 20, 0, 2147483646);
gicp.add("gicp_ransac_iter", int_t, 0, "GICP RANSAC iterations", 30, 0, 2147483646);
gicp.add("gicp_use_recip_corr", bool_t, 0, "GICP use reciprocal correspondences? [true/false]", False)

icpn = gen.add_group("E ICPN registration parameters");
icpn.add("icpn_max_corr_dist", double_t, 0, "ICPN maximum correspondence distance", 3.0, 0.0, 10000000.0);
icpn.add("icpn_ransac_outl_rej_thrd", double_t, 0, "ICPN RANSAC outlier rejection threshold", 0.8, 0.0, 10000000.0);
icpn.add("icpn_trans_eps", double_t, 0, "ICPN transformation epsilon", 0.03, 0.0, 10000000.0);
icpn.add("icpn_eucld_fitn_eps", double_t, 0, "ICPN Euclidean fitness epsilon", 0.03, 0.0, 10000000.0);
icpn.add("icpn_max_iter", int_t, 0, "ICPN maximum iterations", 1000, 0, 2147483646);
icpn.add("icpn_ransac_iter", int_t, 0, "ICPN RANSAC iterations", 300, 0, 2147483646);
icpn.add("icpn_use_recip_corr", bool_t, 0, "ICPN use reciprocal correspondences? [true/false]", False)

sicpn = gen.add_group("E SICPN registration parameters");
sicpn.add("sicpn_number_of_samples", int_t, 0, "SICPN number of heading samples", 8, 0, 100);
sicpn.add("sicpn_max_corr_dist", double_t, 0, "SICPN maximum correspondence distance", 7.0, 0.0, 10000000.0);
sicpn.add("sicpn_ransac_outl_rej_thrd", double_t, 0, "SICPN RANSAC outlier rejection threshold", 0.7, 0.0, 10000000.0);
sicpn.add("sicpn_trans_eps", double_t, 0, "SICPN transformation epsilon", 0.1, 0.0, 10000000.0);
sicpn.add("sicpn_eucld_fitn_eps", double_t, 0, "SICPN Euclidean fitness epsilon", 0.1, 0.0, 10000000.0);
sicpn.add("sicpn_max_iter", int_t, 0, "SICPN maximum iterations", 500, 0, 2147483646);
sicpn.add("sicpn_ransac_iter", int_t, 0, "SICPN RANSAC iterations", 150, 0, 2147483646);
sicpn.add("sicpn_use_recip_corr", bool_t, 0, "SICPN use reciprocal correspondences? [true/false]", False)

exit(gen.generate(PACKAGE, "pcl2map_registration", "pcl2map_registration_dynparam"))
